
/*
 */
#include <Metro.h> //Include Metro library
#include <Servo.h>

enum{
	POS_NTL = 0,
	POS_PSH,
	POS_NUM
};

#define SERVO_NUM	3
static const int servo_pin[SERVO_NUM] = {9, 10, 11};
static const int servo_degs[SERVO_NUM][POS_NUM] = {{65, 55}, {88, 82}, {77, 68
}};
#define COMMAND_DRUM_NUM  4 /* 1コマンドの太鼓数 */


/* INTERVAL */
#define INTERVAL_SERVO_PUSH	200
#define INTERVAL_1PUSH		500	
#define INTERVAL_WAITING	INTERVAL_1PUSH * 4 + 2



/*
 * |--       |--       |--       |--         <- サーボを動かしている間の時間(SERVO_PUSH)
 * |---------|---------|---------|---------  <- 1サイクルのPUSH4セット+その間(1PUSH)
 *  サイクル間の待ち時間(WAITING) ->       ----------------------------------------
 */

enum{
	ST_INI,
	ST_ACT_PSH,
	ST_ACT_REL,
	ST_WAIT
};

/* コマンド */
enum{
	BT_PT = 0,
	BT_PO,
	BT_CH,
	BT_NUM
};
enum{
	COMMAND_ADVANCE = 0,
	COMMAND_POOLSTR,
	COMMAND_ATTACK,
	COMMAND_TYPE_NUM
};
static const int command_pattern[COMMAND_TYPE_NUM][COMMAND_DRUM_NUM] =
{
	{BT_PT, BT_PT, BT_PT, BT_PO},
	{BT_PO, BT_PO, BT_CH, BT_CH},
	{BT_PO, BT_PO, BT_PT, BT_PO},
};

/* 1シナリオのコマンドシーケンス */
static const int scene[2] = {COMMAND_POOLSTR, COMMAND_ATTACK};

typedef struct{
	int state;
	int push_num;	/* 1サイクル中で何回押したかのカウント */
	int now_command;	/* 現在処理中のコマンド */
} T_AUTO_PUSH_DESC;
static T_AUTO_PUSH_DESC desc;

/* Descriptor */
Servo myservo[SERVO_NUM];
Metro m_pushing = Metro(INTERVAL_SERVO_PUSH);
Metro m_cycle_interval = Metro(INTERVAL_WAITING);
Metro m_push_interval = Metro(INTERVAL_1PUSH);

// static functions
static void change_state(int next_state);
static void servo_action(int num);

/**
 * サーボ位置の初期化
 */
static void servo_ini(void)
{
	int i;
	for(i = 0; i < SERVO_NUM; i++){
		myservo[i].attach(servo_pin[i]);
		/* 各サーボのニュートラル位置で初期化 */
		myservo[i].write(servo_degs[i][POS_NTL]);
	}

}

/**
 * 初期化処理
 */
void setup()
{
	servo_ini();
	Serial.begin(115200);

	desc.state = ST_INI;
}

/**
 * メインループ
 */
void loop()
{
	/* アイドル時のステート処理 */
	/* INIステート以外でキャラクタ受信したら初期状態に戻る */
	if(desc.state != ST_INI && Serial.available()){
		Serial.read();
		change_state(ST_INI);
	}

	switch(desc.state){
	case ST_INI:
		if(Serial.available()){
			Serial.read();
			desc.push_num = 0;
			desc.now_command = 0;
			change_state(ST_ACT_PSH);
		}
		break;

	case ST_ACT_PSH:
		if(m_pushing.check()){
			change_state(ST_ACT_REL);
		}
		break;

	case ST_ACT_REL:
		if(m_push_interval.check()){
			if(desc.push_num == COMMAND_DRUM_NUM){
				change_state(ST_WAIT);
			}
			else{
				change_state(ST_ACT_PSH);
			}
		}
		break;

	case ST_WAIT:
		if(m_cycle_interval.check()){
			desc.now_command++;
			/* 次のサイクルへ進む */
			if(desc.now_command >= 2){
				/* 全サイクルを終えた場合は最初に戻る */
				desc.now_command = 0;
			}
			/* num リセット */
			desc.push_num = 0;
			change_state(ST_ACT_PSH);
		}
		break;
	}
}

static void change_state(int next_state)
{
	//Serial.print("st: ");	Serial.print(desc.state);	Serial.print(" -> ");	Serial.println(next_state);

	desc.state = next_state;
	// ステート遷移時の処理を書く
	switch(next_state){
	case ST_INI:
		servo_ini();
		break;
	case ST_ACT_PSH:
		//Serial.print("num: "); Serial.println(desc.push_num);
		servo_action(desc.now_command, desc.push_num, POS_PSH);
		m_pushing.reset();
		m_push_interval.reset();
		break;
	case ST_ACT_REL:
		servo_action(desc.now_command, desc.push_num, POS_NTL);
		desc.push_num++;
		break;
	case ST_WAIT:
		m_cycle_interval.reset();
		break;
	}
}

/**
 * コマンド、PUSH回数と押す/離すの指示から
 * サーボの角度制御を行う
 * サーボ1個のみ制御
 *
 * @param command 何番目のコマンドか
 * @param num 何パターン目か(0-COMMAND_DRUM_NUM)
 * @param push_ntl 0:離す, 1:押す
 */
static void servo_action(int command, int num, int push_ntl)
{
	Servo act_servo;
	int servo_num;

	/* 引数チェック */
	if((command < 0 || command >= sizeof(scene))
	|| (num < 0 || num > COMMAND_DRUM_NUM)
	|| (push_ntl < 0 || push_ntl > 1)){
		return;
	}

	/* 現在のコマンド、パターン数から、制御するサーボオブジェクトを取得 */
	servo_num = command_pattern[scene[command]][num];
	act_servo =	myservo[servo_num];

	/* サーボ番号と PUSH/NTL の指示に従ってサーボ角度を取得、書き込み */
	act_servo.write(servo_degs[servo_num][push_ntl]);
}

